#include <motor/servo_yufeng.h>

namespace ros_canopen_driver {

ServoYufengCurtis::ServoYufengCurtis(std::shared_ptr<rclcpp::Node> node, const std::string name)
{
    node_ = node;
    joint_name = name;
    servo_manufacturer = YUFENG_CURTIS;
    memset(&m_servo_motor, 0, sizeof(SERVOMOTOR_PORT));
    preTargetEnc = -1;    
    encoder = nullptr;
    motorState = Off;
    pc_.timestamp = node_->now().seconds();
}


ServoYufengCurtis::~ServoYufengCurtis()
{
    encoder = nullptr;
}

/*Description: load motor configuration parameters from yaml file*/
bool ServoYufengCurtis::Motor_InitParam()
{
   
    if (!node_->has_parameter(joint_name + "/nodeId")) {
        RCLCPP_ERROR_STREAM(node_->get_logger(), joint_name << " nodeId is invalid");
        return false;
    }
    int nodeId;
    node_->get_parameter(joint_name + "/nodeId", nodeId);
    m_servo_motor.cfg.nodeId = nodeId;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " nodeId is " << nodeId);
    node_->get_parameter_or(joint_name + "/busname", busname, std::string("can0"));
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " busname: " << busname);
    int ctl_mode;
    node_->get_parameter_or(joint_name + "/ctl_mode", ctl_mode, 0);
    m_servo_motor.cfg.ctl_mode = ctl_mode;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " ctl_mode is " << ctl_mode);
    node_->get_parameter_or(joint_name + "/rotateReverse", m_servo_motor.cfg.rotateReverse, false);
    node_->get_parameter_or(joint_name + "/reduction_ratio", m_servo_motor.cfg.reduction_ratio, 1.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " reduction_ratio is " << m_servo_motor.cfg.reduction_ratio);
    node_->get_parameter_or(joint_name + "/encoder_resolution", m_servo_motor.cfg.encoder_resolution, 10000.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " encoder_resolution is " << m_servo_motor.cfg.encoder_resolution);
    node_->get_parameter_or(joint_name + "/use_extra_encoder", use_extra_encoder, false);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " use external encoder: " << use_extra_encoder);
    node_->get_parameter_or(joint_name + "/pos_scale", pos_scale, 1.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " pos_scale: " << pos_scale);
    node_->get_parameter_or(joint_name + "/pos_offset", pos_offset, 0.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " pos_offset: " << pos_offset);
    
    // 速度控制模式配置
    int velocity_mode_acc;
    node_->get_parameter_or(joint_name + "/vModeCfg/acc", velocity_mode_acc, 0);
    m_servo_motor.cfg.vModeCfg.acc = velocity_mode_acc;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " velocity_mode_acc is " << m_servo_motor.cfg.vModeCfg.acc);
    int velocity_mode_dec;
    node_->get_parameter_or(joint_name + "/vModeCfg/dec", velocity_mode_dec, 0);
    m_servo_motor.cfg.vModeCfg.dec = velocity_mode_dec;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " velocity_mode_dec is " << m_servo_motor.cfg.vModeCfg.dec);
    // 位置控制模式配置
    int position_mode_vel;
    node_->get_parameter_or(joint_name + "/pModeCfg/vel", position_mode_vel, 0);
    m_servo_motor.cfg.pModeCfg.vel = position_mode_vel;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " position_mode_vel is " << m_servo_motor.cfg.pModeCfg.vel);
    int position_mode_acc;
    node_->get_parameter_or(joint_name + "/pModeCfg/acc", position_mode_acc, 0);
    m_servo_motor.cfg.pModeCfg.acc = position_mode_acc;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " position_mode_acc is " << m_servo_motor.cfg.pModeCfg.acc);
    int position_mode_dec;
    node_->get_parameter_or(joint_name + "/pModeCfg/dec", position_mode_dec, 0);
    m_servo_motor.cfg.pModeCfg.dec = position_mode_dec;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " position_mode_dec is " << m_servo_motor.cfg.pModeCfg.dec);
    // 家庭配置
    node_->get_parameter_or(joint_name + "/homeNeed", m_servo_motor.cfg.homeNeed, false);
    if (m_servo_motor.cfg.homeNeed) {
        int home_mode;
        node_->get_parameter_or(joint_name + "/hModeCfg/home_mode", home_mode, 0);
        m_servo_motor.cfg.hModeCfg.home_mode = home_mode;
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " home_mode is " << home_mode);
        int home_mode_velH;
        node_->get_parameter_or(joint_name + "/hModeCfg/velH", home_mode_velH, 0);
        m_servo_motor.cfg.hModeCfg.velH = home_mode_velH;
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " home_mode_velH is " << m_servo_motor.cfg.hModeCfg.velH);
        int home_mode_velL;
        node_->get_parameter_or(joint_name + "/hModeCfg/velL", home_mode_velL, 0);
        m_servo_motor.cfg.hModeCfg.velL = home_mode_velL;
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " home_mode_velL is " << m_servo_motor.cfg.hModeCfg.velL);
        int home_mode_acc;
        node_->get_parameter_or(joint_name + "/hModeCfg/acc", home_mode_acc, 0);
        m_servo_motor.cfg.hModeCfg.acc = home_mode_acc;
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " home_mode_acc is " << m_servo_motor.cfg.hModeCfg.acc);
        node_->get_parameter_or(joint_name + "/hModeCfg/homeOffset", m_servo_motor.cfg.hModeCfg.homeOffset, 0);
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " homeOffset is " << m_servo_motor.cfg.hModeCfg.homeOffset);
    }
    /* 2023.8.16 add begin: migrate from HardWareInterface ---> */
    if (servo_type == STEER) { // 获取转向参数
        node_->get_parameter_or(joint_name + "/zero_point_correction", zero_point_correction, 0.0);
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " zero_point_correction: " << zero_point_correction);
    } else if (servo_type == LIFT) { // 获取升降参数
        node_->get_parameter_or(joint_name + "/lift_limit_down", fork_param.lift_limit_down, 0.5);
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " lift_limit_down: " << fork_param.lift_limit_down);
        node_->get_parameter_or(joint_name + "/lift_limit_up", fork_param.lift_limit_up, 1000.0);
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " lift_limit_up: " << fork_param.lift_limit_up);
        node_->get_parameter_or(joint_name + "/lift_up_max_speed_limit", fork_param.lift_up_max_speed_limit, 100.0);
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " lift_max_speed_limit: " << fork_param.lift_up_max_speed_limit);
        node_->get_parameter_or(joint_name + "/lift_down_max_speed_limit", fork_param.lift_down_max_speed_limit, 200.0);
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " lift_max_speed_limit: " << fork_param.lift_down_max_speed_limit);
        fork_param.is_lifting = false;
        fork_param.lift_monitor = node_->create_wall_timer(std::chrono::milliseconds(10), 
            std::bind(&ServoYufengCurtis::servo_checkForkStatus, this));
    }
    /* <--- add end */
    return true;
}


int ServoYufengCurtis::Motor_Init()
{
    if (use_extra_encoder) {
        encoder = encoder::EncoderFactory::generateEncoder(node_,joint_name, encoder::CURTIS_ENCODER);
        encoder->init();
    }
   
    uint8_t nodeId = m_servo_motor.cfg.nodeId;
    if((0 == nodeId) || (nodeId > 127))
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR]motor nodeId is unvaild");
        return -1;       
    }

    // Publisher 初始化
    set_fork_lift_down_speed_pub = node_->create_publisher<std_msgs::msg::UInt8>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_fork_lift_down_speed", 10);
    set_fork_lift_up_speed_pub = node_->create_publisher<std_msgs::msg::UInt8>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_fork_lift_up_speed", 10);
    set_wheel_acc_pub = node_->create_publisher<std_msgs::msg::UInt8>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_wheel_acc", 10);
    set_wheel_dec_pub = node_->create_publisher<std_msgs::msg::UInt8>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_wheel_dec", 10);
    set_target_steer_angle_pub = node_->create_publisher<std_msgs::msg::Int16>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_target_steer_angle", 10);
    set_wheel_speed_pub = node_->create_publisher<std_msgs::msg::Int16>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_wheel_speed", 10);
    set_fork_lift_enable_pub = node_->create_publisher<std_msgs::msg::UInt8>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_valve_enable", 10);
    // Subscription 初始化
    drive_error_sub = node_->create_subscription<std_msgs::msg::UInt8>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_drive_error_code", 
        10, 
        std::bind(&ServoYufengCurtis::drive_error_sub_callback, this, std::placeholders::_1));
    steer_error_sub = node_->create_subscription<std_msgs::msg::UInt8>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_steer_error_code", 
        10, 
        std::bind(&ServoYufengCurtis::steer_error_sub_callback, this, std::placeholders::_1));
    pump_error_sub = node_->create_subscription<std_msgs::msg::UInt8>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_pump_error_code", 
        10, 
        std::bind(&ServoYufengCurtis::pump_error_sub_callback, this, std::placeholders::_1));
    actual_speed_sub = node_->create_subscription<std_msgs::msg::Int16>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_actual_speed", 
        10, 
        std::bind(&ServoYufengCurtis::actual_speed_sub_callback, this, std::placeholders::_1));
    battery_sub = node_->create_subscription<std_msgs::msg::UInt8>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_battery", 
        10, 
        std::bind(&ServoYufengCurtis::battery_sub_callback, this, std::placeholders::_1));
    steer_angle_sub = node_->create_subscription<std_msgs::msg::Int16>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_steer_angle", 
        10, 
        std::bind(&ServoYufengCurtis::steer_angle_sub_callback, this, std::placeholders::_1));
    signals_sub = node_->create_subscription<std_msgs::msg::UInt8>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_signals", 
        10, 
        std::bind(&ServoYufengCurtis::signals_sub_callback, this, std::placeholders::_1));

    RCLCPP_INFO(node_->get_logger(), "node[%d] enable.", nodeId);
    // motor enable
    bool enableFlage = Motor_Enable(true);
    if(!enableFlage)
    {
        return -1;
    }

    // homemachine
    Motor_HomeMachine();

    // ctlmode
    if(m_servo_motor.cfg.ctl_mode == Profiled_Position)
    {
        RCLCPP_INFO(node_->get_logger(), "node[%d] set position mode.", nodeId);
        if (!Motor_InitPmode())
            return -1;
    }
    else if(m_servo_motor.cfg.ctl_mode == Profiled_Velocity)
    {
        RCLCPP_INFO(node_->get_logger(), "node[%d] set velocity mode.", nodeId);
        if(!Motor_InitVmode())
            return -1;
    }
    else
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR]unknown drive mode");
        return -1;
    }
    motorState = Ready;        
    return 0;
}

bool ServoYufengCurtis::Motor_Enable(bool onFlag)
{
   rclcpp::sleep_for(std::chrono::milliseconds(5));
    std_msgs::msg::Int16 wheel_speed_msg;
    wheel_speed_msg.data = 0x00;
    set_wheel_speed_pub->publish(wheel_speed_msg);

   rclcpp::sleep_for(std::chrono::milliseconds(5));
    std_msgs::msg::UInt8  wheel_acc_msg;
    wheel_acc_msg.data = 0x0;
    set_wheel_acc_pub->publish(wheel_acc_msg);

   rclcpp::sleep_for(std::chrono::milliseconds(5));
    std_msgs::msg::UInt8  wheel_dec_msg;
    wheel_dec_msg.data = 0x0;
    set_wheel_dec_pub->publish(wheel_dec_msg);

   rclcpp::sleep_for(std::chrono::milliseconds(5));
    std_msgs::msg::Int16  target_angle_msg;
    target_angle_msg.data = 0x0;
    set_target_steer_angle_pub->publish(target_angle_msg);

   rclcpp::sleep_for(std::chrono::milliseconds(5));
    std_msgs::msg::UInt8   fork_lift_down_speed_msg;
    fork_lift_down_speed_msg.data = 0x0;
    set_fork_lift_down_speed_pub->publish(fork_lift_down_speed_msg);

    return true;
}

bool ServoYufengCurtis::Motor_HomeMachine()
{
    return true;
    /*
    uint16_t statusWord = 0;
    if(!m_servo_motor.cfg.homeNeed) //todo get 609903 check whether the system automatically returns to zero
    {
        RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR]motor does not need to go home");
        return true;
    }
    if(!m_servo_motor.cfg.hModeCfg.home_mode)
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR]motro home mode is not define");
        return false;
    }
    servo_modeSet(Homing);
    servo_homeMethodSet(m_servo_motor.cfg.hModeCfg.home_mode);
    servo_homeOffsetModeSet(0); // todo mode add cfg
    servo_homeOffsetSet(m_servo_motor.cfg.hModeCfg.homeOffset);
    servo_homeSpeedHSet(m_servo_motor.cfg.hModeCfg.velH);
    servo_homeSpeedLSet(m_servo_motor.cfg.hModeCfg.velL);
    servo_homeAccSet(m_servo_motor.cfg.hModeCfg.acc);
    servo_homeStart();
    rclcpp::Time homing_start_time = node_->now();
    rclcpp::Duration(1).sleep();
    do{
        statusWord = pCanopen->getStatusWord(m_servo_motor.cfg.nodeId);
        if((node_->now() - homing_start_time).toSec() > 60) //todo timeout
        {
            break;
        }
    }while(!(statusWord & 0x8000));
    if(statusWord & 0x8000)
    {
        RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR]motro home finished");
    }
    else
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR]motro home timeout");
        Motor_Enable(false); //disable motor
        return false;       
    }
    return true;
    */
}

bool ServoYufengCurtis::Motor_InitVmode()
{
    Motor_SetTargetVelEnc(0);
    return true;
}

bool ServoYufengCurtis::Motor_InitPmode()
{
    return true;
}

bool ServoYufengCurtis::Motor_SwitchMode(int8_t mode)
{
    return true;

    // switch(mode)
    // {
    //     case Profiled_Position:
    //         Motor_InitPmode();
    //         break;
    //     case Profiled_Velocity:
    //         Motor_InitVmode();
    //         break;
    //     case Homing:
    //         break;
    //     default:
    //         RCLCPP_WARN(node_->get_logger(), "undefined control mode");
    //         break;
    // }
    // int8_t actualCtlMode = Motor_GetMode();
    // if(actualCtlMode != mode)
    // {
    //     RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR]failed to set the ctl mode");
    //     return false;
    // }
    // return true;
}

void ServoYufengCurtis::Motor_SetAcc(uint32_t acc)
{
    return;
    // pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 4, 0x6083, 0x00, acc);   
}

void ServoYufengCurtis::Motor_SetDec(uint32_t dec)
{
    return;
    // pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 4, 0x6084, 0x00, dec);
}

void ServoYufengCurtis::Motor_SetPmodeVel(uint32_t vel)
{
    return;
    // pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 4, 0x6081, 0x00, vel);
}



void ServoYufengCurtis::Motor_SetTargetVelRad(double vel)
{
    /* 2023.8.17 add begin:  refactor set pos/vel interface  ---> */
    // RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 1000, "[ServoYufengCurtis] motor[%s] type[%d] state[%d]  target vel: %.4f", joint_name.c_str(), servo_type, Motor_GetState(), vel);

    if(Ready != Motor_GetState())
    {
        if (Motor_GetErrCode()) {
            RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
                1000, "[ServoYufengCurtis] motor[%s] state[%d] not Ready !!!", joint_name.c_str(), Motor_GetState());
            return;
        }
    }

    if (std::isnan(vel) || abs(vel) > 1e9) {
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
            1000, "[ServoYufengCurtis] bad vel value: %.4f", vel);
        Motor_SetTargetVelEnc(0);
        return;
    }

    if (servo_type == LIFT) {
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
            1000, "[ServoYufengCurtis] %s set fork vel: %.4f", joint_name.c_str(), vel);
        servo_setForkVel(vel);
    }

    // STEER servo cannot set vel
    else if (servo_type == NORMAL) {     
        if(m_servo_motor.cfg.reduction_ratio < 1e-6)
        {
            RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
                1000, "[ServoYufengCurtis] motor[%s] reduction_ratio not defined.", joint_name.c_str());
            return;
        }

        double vel_enc = 30 * vel * m_servo_motor.cfg.reduction_ratio / M_PI;
        int target_enc = (int)vel_enc;
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
            1000, "[ServoYufengCurtis] %s set vel[%.4f]  enc[%d]", joint_name.c_str(), vel, target_enc);
        Motor_SetTargetVelEnc(target_enc);
    }
    /* <--- add end */
}

void ServoYufengCurtis::Motor_SetTargetVelEnc(int vel)
{
    // if (vel == 0)
    // {
    //     std_msgs::msg::UInt8 wheel_dec_msg;
    //     wheel_dec_msg.data = 0x1;
    //     set_wheel_dec_pub->publish(wheel_dec_msg);

    //     std_msgs::msg::UInt8 wheel_acc_msg;
    //     wheel_acc_msg.data = 0x0;
    //     set_wheel_acc_pub->publish(wheel_acc_msg);
    // }
    // else
    // {
    //     std_msgs::msg::UInt8 wheel_acc_msg;
    //     wheel_acc_msg.data = 0x1;
    //     set_wheel_acc_pub->publish(wheel_acc_msg);

    //     std_msgs::msg::UInt8 wheel_dec_msg;
    //     wheel_dec_msg.data = 0x0;
    //     set_wheel_dec_pub->publish(wheel_dec_msg);
    // }
 
    std_msgs::msg::UInt8 wheel_acc_msg;
    wheel_acc_msg.data = 0x1;
    set_wheel_acc_pub->publish(wheel_acc_msg);
    set_wheel_dec_pub->publish(wheel_acc_msg);

    if (vel > 4000)
        vel = 4000;
    if (vel < -4000)
        vel = -4000;

    std_msgs::msg::Int16 wheel_speed_msg;
    wheel_speed_msg.data = vel;
    set_wheel_speed_pub->publish(wheel_speed_msg);
}

void ServoYufengCurtis::Motor_SetTargetPosRad(double pos)
{
    // RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 1000, "[ServoYufengCurtis] motor[%s] type[%d] state[%d]  target pos: %.4f", joint_name.c_str(), servo_type, Motor_GetState(), pos);
    if(Ready != Motor_GetState())
    {
        if (Motor_GetErrCode()) {
            RCLCPP_INFO(node_->get_logger(), "[ServoYufengCurtis] motor[%s] state[%d] not Ready !!!", joint_name.c_str(), Motor_GetState());
            return;
        }
    }

    if((m_servo_motor.cfg.reduction_ratio < 1e-6) || !m_servo_motor.cfg.encoder_resolution)
    {
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
            1000, "[ServoYufengCurtis] motor[%s] reduction_ratio||encoder_resolution is 0.", joint_name.c_str());
        return;
    }
    
    if (servo_type == STEER) {
        int targetEnc = pos * 180.0 * ratio * m_servo_motor.cfg.reduction_ratio / M_PI + zero_point_correction;
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
            1000, "[ServoYufengCurtis] %s set pos[%.4f]  enc[%d]", joint_name.c_str(), pos, targetEnc);
        if (targetEnc > 12000 || targetEnc < -12000) {
            targetEnc = 0;
            RCLCPP_ERROR_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
                1000, "[ServoYufengCurtis] %s bad target pos, set 0", joint_name.c_str());
        }
        Motor_SetTargetPosEnc(targetEnc);
    }
    /* <--- modify end */
}

void ServoYufengCurtis::Motor_SetTargetPosEnc(int pos)
{
    int sd = (pos != 0 ? (pos * -1) : pos);
    std_msgs::msg::Int16 target_angle_msg;
    target_angle_msg.data = sd;
    set_target_steer_angle_pub->publish(target_angle_msg);
}

double ServoYufengCurtis::Motor_GetCurrentVel()
{
    int vel;
    double retVel;
    // const double temp  = 30 / M_PI * m_servo_motor.cfg.reduction_ratio * 512 * m_servo_motor.cfg.encoder_resolution  / 1875;
   
    if (servo_type == NORMAL) {
        retVel = (double)pc_.actual_speed * M_PI / (30.0 * m_servo_motor.cfg.reduction_ratio);
        // vel = pc_.actual_speed;
    }
    else {
        retVel = 0;
    }

    return retVel;
}



/* 2023.8.16 add begin:  refactor pos calculation  ---> */
double ServoYufengCurtis::Motor_GetCurrentPos()
{
    int64_t enc;
    double pos;
    if (!use_extra_encoder) {
        enc = servo_encoderGet();
        pos = (enc - zero_point_correction) * M_PI / (ratio * m_servo_motor.cfg.reduction_ratio * 180);
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
            1000, "[ServoYufengCurtis] [%s] get pos from servo: %.4f  enc: %d  zero: %.2f  ratio: %.2f reduct: %.2f", 
            joint_name.c_str(), pos, enc, zero_point_correction, ratio, m_servo_motor.cfg.reduction_ratio);
    }
    else {
        if (encoder && Motor_GetState() == Ready) {
            float scale = pos_scale, offset = pos_offset;
            node_->get_parameter(joint_name + "/pos_scale", scale);
            node_->get_parameter(joint_name + "/pos_offset", offset);
            if (std::fabs(scale - pos_scale) > 1e-6f || std::fabs(offset - pos_offset) > 1e-6f) {
                pos_scale = scale;
                pos_offset = offset;
                RCLCPP_INFO_STREAM(node_->get_logger(),
                    "Reset params [" << joint_name << "] pos_offset: " << pos_offset << ", pos_scale:" << pos_scale);
            }

            double enc_value = (encoder->getTurns() * encoder->max_value + encoder->getValue()) / ratio;
            pos = enc_value * pos_scale + pos_offset;

            pc_.timestamp = encoder->timestamp_;
        } else
            pos = 0.0;
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
            1000, "[ServoYufengCurtis] [%s] get pos from encoder: %.4f", joint_name.c_str(), pos);
    }

    return pos;
}

double ServoYufengCurtis::Motor_GetCurrent()
{
    return 0.0;
}

/* <--- add end */


/*Description: get the motor encoder*/
int ServoYufengCurtis::servo_encoderGet()
{
    if (servo_type == STEER)
    {
        int angle = pc_.steer_angle;
        return (angle != 0 ? angle * -1 : angle);
    }
    else if (servo_type == NORMAL)
    {
        return 0;
    }
    else
        return 0;
}


uint16_t ServoYufengCurtis::Motor_GetStatusWord()
{
    return 0;
}

uint16_t ServoYufengCurtis::Motor_GetErrCode()
{
    uint16_t errCode = (uint8_t)pc_.drive_error | (uint8_t)pc_.steer_error | (uint8_t)pc_.pump_error;
    int drive_error = pc_.drive_error;
    int steer_error = pc_.steer_error;
    int pump_error = pc_.pump_error;

    if(Ready == Motor_GetState())
    {
        if(errCode)
        {
            RCLCPP_WARN_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
                1000, "[ServoYufengCurtis] drive_erros: %d  steer_error: %d  pump_error: %d", drive_error, steer_error, pump_error);
            motorState = Error;
        }
    }
    else {
        if (Error == Motor_GetState() && errCode == 0) {
            motorState = Ready;
            RCLCPP_WARN_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
                1000, "[ServoYufengCurtis] errcode clear");   
        }
    }
    return errCode;
}

int8_t ServoYufengCurtis::Motor_GetMode()
{
    // return pCanopen->getActualCtlMode(m_servo_motor.cfg.nodeId);
    return 0;
}


void ServoYufengCurtis::Motor_Brake(bool on)
{
    return;
}

void ServoYufengCurtis::Motor_Shutdown()
{
    // pCanopen->nmtSend(m_servo_motor.cfg.nodeId, Canopen::State::Stopped);
    // TODO: NMT STOP
    if (servo_type == LIFT) {
        fork_param.lift_monitor->cancel();
    }
}

bool ServoYufengCurtis::Motor_Halt()
{
    motorState = Halt;
    // TODO
    return true;
}

bool ServoYufengCurtis::Motor_Recover()
{
    if(motorState == Error || motorState == Halt) {
        motorState = Ready;
    }
    // TODO
    return true;
}


std::string ServoYufengCurtis::Motor_Err2userErr(const uint16_t& motorErr)
{
    std::string errInfo;
    switch(motorErr)
    {
        case 0x0000: errInfo = "no err"; break;
        case 0x7380: errInfo = "Encoder ABZ signal error"; break;
        case 0x7331: errInfo = "Encoder communication error"; break;
        case 0x7381: errInfo = "Encoder UVW signal error"; break;
        case 0x7320: errInfo = "Encoder internal error"; break;
        case 0x7305: errInfo = "Encoder count error"; break;
        case 0x7330: errInfo = "Encoder CRC check error"; break;
        case 0x4210: errInfo = "Driver overtemperature"; break;
        case 0x3210: errInfo = "Driver voltage is too high"; break;
        case 0x3220: errInfo = "Driver voltage is too low."; break;
        case 0x2320: errInfo = "Driver output short circuit"; break;
        case 0x7110: errInfo = "Driver brake resistance is abnormal"; break;
        case 0x8611: errInfo = "Following error"; break;
        case 0x5112: errInfo = "The logical voltage is too low."; break;
        case 0x2350: errInfo = "Motor overload"; break;
        case 0x8A80: errInfo = "The input pulse frequency is too high"; break;
        case 0x4310: errInfo = "Driver overtemperature"; break;
        case 0x7122: errInfo = "Motor UVW logic combination error"; break;
        case 0x6310: errInfo = "EEPROM data error"; break;
        case 0x5210: errInfo = "Current sensor fault"; break;
        case 0x6010: errInfo = "The watchdog reports error"; break;
        case 0x6011: errInfo = "Invalid interrupt occur"; break;
        case 0x7400: errInfo = "MCU fault"; break;
        case 0x6320: errInfo = "Motor type error"; break;
        case 0x6321: errInfo = "Motor phase loss"; break;
        case 0x5443: errInfo = "The alarm is pre-enabled"; break;
        case 0x5442: errInfo = "Positive limit reported error"; break;
        case 0x5441: errInfo = "Negative limit reported error"; break;
        case 0x6012: errInfo = "SPI communication failure"; break;
        case 0x8100: errInfo = "Communication error"; break;
        case 0x81FF: errInfo = "Communication timeout"; break;
        case 0x8A81: errInfo = "Full closed loop check error"; break;
        case 0x7382: errInfo = "Master encoder ABZ error"; break;
        case 0x7306: errInfo = "Master encoder count error"; break;
        case 0x5530: errInfo = "device software reset"; break;
        case 0xFF42: errInfo = "Battery is discharged"; break;
        case 0xFF45: errInfo = "Current Gain under acquisition"; break;
        case 0xFF50: errInfo = "Forward and Reverse request direction together"; break;
        case 0x5301: errInfo = "Pal Circuit KO"; break;
        case 0x1000: errInfo = "generic warning"; break;
        case 0xFFF4: errInfo = "When the input B3 is opened"; break;
        case 0x3223: errInfo = "A11output broken/DC link under-voltage No.3 (manufacturer specific)"; break;
        case 0x4211: errInfo = "sensor temperatur device broken (manufacturer specific)"; break;
        default:     errInfo = "unknown warning"; break;
    }
    return errInfo;
}


MotorState ServoYufengCurtis::Motor_GetState() 
{
    return motorState;
}




/* 2023.8.18 add begin: migrate from HardwareInterface ---> */
#define NOMOVE   100
#define UPWORD   1
#define DOWNWORD 0


void ServoYufengCurtis::servo_doForkLift(bool up, int vel)
{
    std_msgs::msg::UInt8 enable;
    std_msgs::msg::UInt8 fork_lift_speed_msg;
    std_msgs::msg::UInt8 fork_lift_zero_msg;
    fork_lift_zero_msg.data = 0;

    if (up && ((vel < 0) || (vel > fork_param.lift_up_max_speed_limit)))
    {
        RCLCPP_WARN_STREAM(node_->get_logger(), "do forkLift upward param is invalid");
        return;
    }
    if ((!up && ((vel < 0) || (vel > fork_param.lift_down_max_speed_limit))))
    {
        RCLCPP_WARN_STREAM(node_->get_logger(), "do forkLift downward param is invalid");
        return;
    }
    
    fork_lift_speed_msg.data = vel;

    if (vel == 0) {
        enable.data = 0;
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
            1000, "do forkLift stop.");
        set_fork_lift_up_speed_pub->publish(fork_lift_speed_msg);
        set_fork_lift_down_speed_pub->publish(fork_lift_speed_msg); 
    } else {
        if (up)
        {
            enable.data = 0x1;
            set_fork_lift_down_speed_pub->publish(fork_lift_zero_msg);
            set_fork_lift_up_speed_pub->publish(fork_lift_speed_msg);
        }
        else
        {
            enable.data = 0x0;
            set_fork_lift_up_speed_pub->publish(fork_lift_zero_msg);
            set_fork_lift_down_speed_pub->publish(fork_lift_speed_msg); 
        }
    }

    set_fork_lift_enable_pub->publish(enable);
}


void ServoYufengCurtis::servo_checkForkStatus()
{
    if (servo_type == LIFT) { 
        bool needstop = false;
        // limit up
        // if (joint_position_[servo_[i].index] >= servo_[i].forklift->lift_limit_up && 
        if (Motor_GetCurrentPos() >= fork_param.lift_limit_up && 
            fork_param.is_lifting && fork_param.cur_order == UPWORD && 
            fork_param.cur_speed > 0) {
            RCLCPP_INFO_STREAM(node_->get_logger(), "Forklift up limit stop!");
            needstop = true;
        }
        // limit down
        // if (joint_position_[servo_[i].index] <= fork_param.lift_limit_down &&
        if (Motor_GetCurrentPos() <= fork_param.lift_limit_down &&
            fork_param.is_lifting && fork_param.cur_order == DOWNWORD && 
            fork_param.cur_speed > 0 ) {
            RCLCPP_INFO_STREAM(node_->get_logger(), "Forklift down limit stop!");
            needstop = true;
        }
        //  timer set overflow 500ms
        if ((node_->now() - fork_param.last_action_time >= rclcpp::Duration::from_seconds(0.5)) && 
            fork_param.is_lifting && fork_param.cur_speed > 0) {
            RCLCPP_INFO_STREAM(node_->get_logger(), "Forklift timer overflow stop!");
            needstop = true;
        }

        if (needstop) {
            // stop lift
            bool order = fork_param.cur_order;
            fork_param.is_lifting = false;
            fork_param.cur_speed = 0;
        
            // RCLCPP_INFO_STREAM(node_->get_logger(), "Forklift stop!");
            servo_doForkLift(order, 0);
            fork_param.cur_order = NOMOVE;
        }
    }
}

bool ServoYufengCurtis::servo_setForkVel(const int vel)
{
    int sp = abs(vel);
    bool order = false;
    static bool old_order = false;

    RCLCPP_INFO_STREAM(node_->get_logger(), "receive cmd fork lift: [" << order << ", " << vel << "]");
    
    if (vel > 0) { // just to determine whether the limit is exceeded
        order = UPWORD;
        if (Motor_GetCurrentPos() > fork_param.lift_limit_up) {
            RCLCPP_INFO_STREAM(node_->get_logger(), 
                "warn inoperative: fork lift up pos: " << Motor_GetCurrentPos()<< " > limit: " << fork_param.lift_limit_up);
            sp = 0;
        }
        old_order = order;
    } else if (vel < 0) {
        order = DOWNWORD;   
        if (Motor_GetCurrentPos() < fork_param.lift_limit_down) {
            RCLCPP_INFO_STREAM(node_->get_logger(), 
                "warn inoperative: fork lift dwon pos: " << Motor_GetCurrentPos()<< " > limit: " << fork_param.lift_limit_down);
            sp = 0;
        }
        old_order = order;
    } else if (vel == 0) {
        sp = 0;
        order = old_order;
    }

    if (sp > 100)
        sp = 100;

    if (vel > 0)
        sp = fork_param.lift_up_max_speed_limit / 100.0 * sp;
    else if (vel < 0)
        sp = fork_param.lift_down_max_speed_limit / 100.0 * sp;

    if (sp == 0) 
        fork_param.is_lifting = false;
    else
        fork_param.is_lifting = true;

    
    RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
        1000, "[ServoYufengCurtis] doforklift  order: %d  vel: %d", order, sp);
    servo_doForkLift(order, sp);
    fork_param.last_action_time = node_->now();
    fork_param.cur_order = order;
    fork_param.cur_speed = sp;
 
    
    return true;
}


void ServoYufengCurtis::drive_error_sub_callback(const std_msgs::msg::UInt8::ConstSharedPtr& msg)
{
    pc_.drive_error = msg->data;
}

void ServoYufengCurtis::steer_error_sub_callback(const std_msgs::msg::UInt8::ConstSharedPtr& msg)
{
    pc_.steer_error = msg->data;
}

void ServoYufengCurtis::pump_error_sub_callback(const std_msgs::msg::UInt8::ConstSharedPtr& msg)
{
    pc_.pump_error = msg->data;
}

void ServoYufengCurtis::signals_sub_callback(const std_msgs::msg::UInt8::ConstSharedPtr& msg)
{
    pc_.signals = msg->data;
}

void ServoYufengCurtis::actual_speed_sub_callback(const std_msgs::msg::Int16::ConstSharedPtr& msg)
{
    pc_.actual_speed = msg->data;
}

void ServoYufengCurtis::battery_sub_callback(const std_msgs::msg::UInt8::ConstSharedPtr& msg)
{
    pc_.battery = msg->data;
}       

void ServoYufengCurtis::steer_angle_sub_callback(const std_msgs::msg::Int16::ConstSharedPtr& msg)
{
    pc_.steer_angle = msg->data;
}   


/* <--- add end */




}

